package geom;

public class Quat {
	public static final float EPSILON = 0.00001f;
	float w, x, y, z;
	
	public Quat()
	{
		reset();
	}
	
	public Quat(float w, float x, float y, float z)
	{
		this.w = w;
		this.x = x;
		this.y = y;
		this.z = z;
	}
	
	public Quat(Quat q)
	{
		set(q);
	}
	
	public void reset()
	{
		w = 1.0f;
		x = 0.0f;
		y = 0.0f;
		z = 0.0f;
	}
	
	public void set(float w, Vector v)
	{
		this.w = w;
		x = v.x;
		y = v.y;
		z = v.z;
	}
	
	public void set(float w, float x, float y, float z)
	{
		this.w = w;
		this.x = x;
		this.y = y;
		this.z = z;
	}
	public void set(Quat q)
	{
		w = q.w;
		x = q.x;
		y = q.y;
		z = q.z;
	}
	
	public void normalize()
	{
		float square = x * x + y * y + z * z + w * w;
		float dist = (square > 0.0f) ? (1.0f / (float) Math.sqrt(square)) : 1.0f;
		
		x *= dist;
		y *= dist;
		z *= dist;
		w *= dist;
	}
	
	public static Quat mul(Quat q1, Quat q2)
	{
		Quat res = new Quat();
		res.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
		res.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
		res.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
		res.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
		return res;
	}
	
	public AxisAngle getValue()
	{
		// transforming this quat into an angle and an axis vector...
		
		float sa = (float) Math.sqrt(1.0f - w * w);
		if (sa < EPSILON)
		{
			sa = 1.0f;
		}

		return new AxisAngle(x / sa, y / sa, z / sa, (float) Math.acos(w) * 2.0f);
	}
	
	public String toString() {
		StringBuffer sb = new StringBuffer();
		sb.append("{");
		sb.append(w);
		sb.append(", ");
		sb.append(x);
		sb.append(", ");
		sb.append(y);
		sb.append(", ");
		sb.append(z);
		sb.append("}");
		return sb.toString();
	}
	
	public float getW(){
		return w;
	}
	
	public Vector getVector(){
		return new Vector(x, y, z);
	}
}
